/*
 * @Author: EltonLi lijiaming@uavi-tech.com
 * @Date: 2023-09-07 18:32:02
 * @LastEditors: LVGRAPE
 * @LastEditTime: 2025-07-02 16:51:54
 * @FilePath: \zino-fc-v4\ZINO\filter\filter.h
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
/***
 * @Author: LVGRAPE
 * @Date: 2023-05-18 11:21:11
 * @LastEditTime: 2023-12-03 18:17:30
 * @LastEditors: LVGRAPE
 * @Description:
 * @FilePath: \zino-fc-v4\ZINO\filter\filter.h
 * @可以输入预定的版权声明、个性签名、空行等
 */

#ifndef __FILTERS_H_
#define __FILTERS_H_


#include <stdint.h>
#include <stdbool.h>

#ifndef UNUSED
#define UNUSED(x) (void)(x)
#endif

struct filter_s;
typedef struct filter_s filter_t;
typedef struct pt1Filter_s {
    float state;
    float k;
} pt1Filter_t;
typedef struct
{
    float Last_P;//上次估算协方差 不可以为0 ! ! ! ! !
    float Now_P;//当前估算协方差
    float out;//卡尔曼滤波器输出
    float Kg;//卡尔曼增益
    float Q;//过程噪声协方差
    float R;//观测噪声协方差
}Kalman_t;
typedef struct
{
    float mesure;
    float mesure_last;
    float K;
    float Estimate;
    float Estimate_last;
    float Error_est;
    float Error_mes;
    float Error_est_last;
}Kalman_cal_t;

typedef struct
{
	float a1;
	float a2;
	float b0;
	float b1;
	float b2;
	float delay_element_1;
	float delay_element_2;
} lpf2pData;
typedef struct slewFilter_s {
    float state;
    float slewLimit;
    float threshold;
} slewFilter_t;

/* this holds the data required to update samples thru a filter */
typedef struct biquadFilter_s {
    float b0, b1, b2, a1, a2;
    float x1, x2, y1, y2;
} biquadFilter_t;
typedef struct laggedMovingAverage_s {
    uint16_t movingWindowIndex;
    uint16_t windowSize;
    float movingSum;
    float *buf;
    bool primed;
} laggedMovingAverage_t;

typedef enum {
    FILTER_PT1 = 0,
    FILTER_BIQUAD,
} lowpassFilterType_e;

typedef enum {
    FILTER_LPF,    // 2nd order Butterworth section
    FILTER_NOTCH,
    FILTER_BPF,
} biquadFilterType_e;

typedef float (*filterApplyFnPtr)(filter_t *filter, float input);

float nullFilterApply(filter_t *filter, float input);

void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);
void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
void biquadFilterUpdateLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);

float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
float biquadFilterApply(biquadFilter_t *filter, float input);
float filterGetNotchQ(float centerFreq, float cutoffFreq);

void laggedMovingAverageInit(laggedMovingAverage_t *filter, uint16_t windowSize, float *buf);
float laggedMovingAverageUpdate(laggedMovingAverage_t *filter, float input);

float pt1FilterGain(float f_cut, float dT);
void pt1FilterInit(pt1Filter_t *filter, float k);
void pt1FilterUpdateCutoff(pt1Filter_t *filter, float k);
float pt1FilterApply(pt1Filter_t *filter, float input);
float pt1FilterApply2(pt1Filter_t *filter, float input, float f_cut, float dT);
void slewFilterInit(slewFilter_t *filter, float slewLimit, float threshold);
float slewFilterApply(slewFilter_t *filter, float input);

float KalmanFilter(Kalman_t *kfp,float input);
void KalmanFilterInit(Kalman_t *kfp);
void KalmanFilterInitParam(Kalman_t *kfp, float q, float r);
void lpf2pInit(lpf2pData* lpfData, float sample_freq, float cutoff_freq);
void lpf2pSetCutoffFreq(lpf2pData *lpfData, float sample_freq, float cutoff_freq);
float lpf2pApply(lpf2pData* lpfData, float sample);
float nullFilterApply(filter_t *filter, float input);
#endif
